/* 
 * File:   linear_regression.cpp
 * Author: Athanasios Polydoros
 * 
 * Created on 03 July 2013, 20:35
 */

#include "linear_regression.h"

 LinearRegression::LinearRegression(int numSensors, int numMotors) 
{
     //members' initialization
     inputNum=numMotors;
     outputNum=numSensors;
     //+1 for the bias term
     inputs.set((inputNum)+1,1);
    //initialize bias for w0
     inputs.val(0,0)=1;
     regression_coeff.set(inputs.getM(),outputNum);
     error.set(inputNum,0);
     desiredOut.set(outputNum,1);
     //initialize the inverse correlationa as diagonal matrix with large elements
     invCorrelation.set(inputs.getM(),inputs.getM());
     invCorrelation.toId();
     invCorrelation=invCorrelation*(1/0.001);
     forgettFactor=1;
     
}

LinearRegression::~LinearRegression() {
}

void LinearRegression::setState(Matrix motors)
{
    
    
        for (int i=1; i<inputNum+1; i++)
        {
        inputs.val(i,0)=motors.val(i-1,0);    
        
        }
   
     
}

Matrix LinearRegression::getModelMatrix ( Matrix unused)
{
   
        return regression_coeff.rows(1,regression_coeff.getM()-1);
   
 
    
}
void LinearRegression::setDesiredOut(Matrix sensors)
 {
 
    desiredOut=sensors;
    
 }
void LinearRegression::updateWeights()
 {
   //denominator of recursive least square rule
    double denominator;
    denominator=((inputs^T)*invCorrelation*inputs).val(0,0);
   
    denominator=1/(denominator+forgettFactor);
    
    invCorrelation=invCorrelation-((((invCorrelation*inputs)*(inputs^T))*invCorrelation)*
                    denominator);
   
    
    invCorrelation=invCorrelation*(1/forgettFactor);
    //update regression coefficients
    regression_coeff=regression_coeff+(invCorrelation*inputs*(error^T));
    
    
 }
Matrix LinearRegression::predict()
{
   //find error
    predictedOut=(inputs^T)*regression_coeff;
    error=desiredOut-(predictedOut^T);
    //update weights based on this error
    updateWeights();
    return predictedOut^T;
}